#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include "nlink_parser/LinktrackAnchorframe0.h"
#include "nlink_parser/LinktrackTagframe0.h"
#include "nlink_parser/LinktrackNodeframe2.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/NavSatFix.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Vector3Stamped.h"
#include "geometry_msgs/Quaternion.h"
#include "nav_msgs/Path.h"
#include <nav_msgs/Odometry.h>
#include "std_msgs/Float32.h"

#include <math.h>

using namespace std;

sensor_msgs::Imu imu_car;
geometry_msgs::Vector3Stamped euler_angels;//欧拉角/*!< Roll, Pitch and Yaw angles in rad. */
float pos_uwb[3]={0,0,0};
double pi = 3.1415926535897932;
geometry_msgs::Quaternion yaw_orientation;
float yaw;
float yaw_offset;   

void imu_subCallback(const sensor_msgs::Imu::ConstPtr& imu_msg)
{
    imu_car.orientation.x = imu_msg->orientation.x;
    imu_car.orientation.y = imu_msg->orientation.y;
    imu_car.orientation.z = imu_msg->orientation.z;
    imu_car.orientation.w = imu_msg->orientation.w;
    // imu_car.orientation_covariance = imu_msg->orientation_covariance;
    // imu_car.angular_velocity.x = imu_msg->angular_velocity.x ;
    // imu_car.angular_velocity.y = imu_msg->angular_velocity.y ;
    // imu_car.angular_velocity.z = imu_msg->angular_velocity.z ;
    // imu_car.angular_velocity_covariance = imu_msg->angular_velocity_covariance ;
    // imu_car.linear_acceleration.x = imu_msg->linear_acceleration.x;
    // imu_car.linear_acceleration.y = imu_msg->linear_acceleration.y;
    // imu_car.linear_acceleration.z = imu_msg->linear_acceleration.z;
    // imu_car.linear_acceleration_covariance = imu_msg->linear_acceleration_covariance ;
}

void euler_angels_subCallback(const geometry_msgs::Vector3Stamped::ConstPtr& euler_angels_msg)
{/*!< Roll, Pitch and Yaw angles in rad. */
    euler_angels.vector.x = euler_angels_msg->vector.x;
    euler_angels.vector.y = euler_angels_msg->vector.y;
    euler_angels.vector.z = euler_angels_msg->vector.z;
}

void cb_uwb(const nlink_parser::LinktrackNodeframe2::ConstPtr& msg)
{
	nlink_parser::LinktrackNodeframe2 uwb_data;
	uwb_data = *msg;
	pos_uwb[0] =  uwb_data.pos_3d[0];
	pos_uwb[1] =  uwb_data.pos_3d[1];
	pos_uwb[2] =  uwb_data.pos_3d[2];
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "uwb_mag_odom");
    ros::NodeHandle n("");
    ros::NodeHandle n_private("~"); //从参数服务器服务器获取参数需要在私有空间
    
    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
    ros::Publisher yaw_pub = n.advertise<std_msgs::Float32>("yaw", 50);
    //tf::TransformBroadcaster odom_broadcaster;
    ros::Subscriber imu_sub = n.subscribe("imu", 10, imu_subCallback);
    ros::Subscriber euler_angels_sub = n.subscribe("euler_angels", 10, euler_angels_subCallback);
    ros::Subscriber uwb_sub = n.subscribe("nlink_linktrack_nodeframe2",10,cb_uwb);

    //赋初值 
    yaw_orientation.x = 0;
    yaw_orientation.y = 0;
    yaw_orientation.z = 0;
    yaw_orientation.w = 1;
    n_private.param<float>("mag_yaw_offset",yaw_offset, 0); //degree
    std::cout << "yaw_offset: " << yaw_offset << std::endl;
    yaw = -euler_angels.vector.z + yaw_offset*pi/180; //加上一个附加矫正值，并反号变成向上为正


    // 设置循环的频率
    ros::Rate loop_rate(50);

    while(ros::ok())
    {
        ros::spinOnce();

       // ROS_INFO("Roll is %f",euler_angels.vector.x/pi*180);
       // ROS_INFO("Pitch is %f",euler_angels.vector.y/pi*180);
       // ROS_INFO("Yaw is %f",euler_angels.vector.z/pi*180);

	yaw = -euler_angels.vector.z + yaw_offset*pi/180; //加上一个附加矫正值，并反号变成向上为正

	tf::Quaternion q;
	q = tf::createQuaternionFromRPY(0.0, 0.0, yaw);
	yaw_orientation.x = q[0];
	yaw_orientation.y = q[1];
	yaw_orientation.z = q[2];
	yaw_orientation.w = q[3];
        //发布消息
        nav_msgs::Odometry odom;
        odom.header.stamp = ros::Time::now();
        odom.header.frame_id = "map";
        odom.child_frame_id = "car_base_link";

        odom.pose.pose.position.x = pos_uwb[0];
        odom.pose.pose.position.y = pos_uwb[1];
        odom.pose.pose.position.z = pos_uwb[2];
        odom.pose.pose.orientation = yaw_orientation;

        // odom.twist.twist.linear.x = vx;
        // odom.twist.twist.linear.y = vy;
        // odom.twist.twist.angular.z = vth;

        odom_pub.publish(odom);

	std_msgs::Float32 yaw_msgs;
	yaw_msgs.data = yaw*180/pi; //degree
	yaw_pub.publish(yaw_msgs);

        loop_rate.sleep();

    }
    return 0;
}
